{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cal3s2-title"
      },
      "source": [
        "# Cal3_S2"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cal3s2-colab-badge"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/geometry/doc/Cal3_S2.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cal3s2-intro"
      },
      "source": [
        "A `Cal3_S2` represents the simple 5-parameter camera calibration model {cite:p}`https://doi.org/10.1017/CBO9780511811685`[^f1]. This model includes parameters for the focal lengths ($f_x, f_y$), the skew factor ($s$), and the principal point ($u_0, v_0$). It does not model lens distortion. The calibration matrix $K$ is defined as:\n",
        "\n",
        "$$\n",
        "K = \\begin{bmatrix} f_x & s & u_0 \\\\ 0 & f_y & v_0 \\\\ 0 & 0 & 1 \\end{bmatrix}\n",
        "$$\n",
        "\n",
        "This model is based on the assumption of a pinhole camera model. The main purpose of this model is for instrinsic conversions between image pixel coordinates $(u, v)$ in the image sensor frame and the normalized image coordinates $(x, y)$. The normalized image coordinates (also called intrinsic coordinates) are coordinates on a canonical image plane that is unit focal length away from the aperture point. In other words, given any 3D point $(x_c, y_c, z_c)$ based on the camera frame coordinates, a `Cal3_S2` model can handle conversions between $(x, y) = \\left(\\frac{x_c}{z_c}, \\frac{y_c}{z_c}\\right)$ and $(u, v)$, where\n",
        "\n",
        "$$\n",
        "\\begin{align*}\n",
        "u &= f_x\\cdot x + s\\cdot y + u_0 \\\\\n",
        "v &= f_y\\cdot y + v_0\n",
        "\\end{align*}\n",
        "$$\n",
        "\n",
        "<!-- The following will not be rendered as literal text by MyST -->\n",
        "[^f1]: See ch. 6 $\\S$ 6.1, pp. 153–157."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "cal3s2-pip-install",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "cal3s2-imports",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Cal3_S2, Point2"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cal3s2-initialization-header"
      },
      "source": [
        "## Initialization"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "A `Cal3_S2` can be initialized in several ways:"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "cal3s2-initialization-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Default constructor (fx=1, fy=1, s=0, u0=0, v0=0):\n",
            "Cal3_S2[\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "fx: 1.0, fy: 1.0, s: 0.0, u0: 0.0, v0: 0.0\n",
            "\n",
            "From parameters (fx, fy, s, u0, v0):\n",
            "Cal3_S2[\n",
            "\t1500, 0.1, 320;\n",
            "\t0, 1600, 240;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "fx: 1500.0, fy: 1600.0, s: 0.1, u0: 320.0, v0: 240.0\n",
            "\n",
            "From a 5-vector [fx, fy, s, u0, v0]:\n",
            "Cal3_S2[\n",
            "\t1500, 0.1, 320;\n",
            "\t0, 1600, 240;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "fx: 1500.0, fy: 1600.0, s: 0.1, u0: 320.0, v0: 240.0\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Default constructor: fx=1, fy=1, s=0, u0=0, v0=0\n",
        "cal0 = Cal3_S2()\n",
        "print(\"Default constructor (fx=1, fy=1, s=0, u0=0, v0=0):\")\n",
        "print(cal0)\n",
        "print(f\"fx: {cal0.fx()}, fy: {cal0.fy()}, s: {cal0.skew()}, u0: {cal0.px()}, v0: {cal0.py()}\\n\")\n",
        "\n",
        "# From individual parameters: fx, fy, s, u0, v0\n",
        "fx, fy, s, u0, v0 = 1500.0, 1600.0, 0.1, 320.0, 240.0\n",
        "cal1 = Cal3_S2(fx, fy, s, u0, v0)\n",
        "print(\"From parameters (fx, fy, s, u0, v0):\")\n",
        "print(cal1)\n",
        "print(f\"fx: {cal1.fx()}, fy: {cal1.fy()}, s: {cal1.skew()}, u0: {cal1.px()}, v0: {cal1.py()}\\n\")\n",
        "\n",
        "# From a 5-vector [fx, fy, s, u0, v0]\n",
        "cal_vector = np.array([1500.0, 1600.0, 0.1, 320.0, 240.0])\n",
        "cal2 = Cal3_S2(cal_vector)\n",
        "print(\"From a 5-vector [fx, fy, s, u0, v0]:\")\n",
        "print(cal2)\n",
        "print(f\"fx: {cal2.fx()}, fy: {cal2.fy()}, s: {cal2.skew()}, u0: {cal2.px()}, v0: {cal2.py()}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "Additionally, you can construct a calibration model with the field-of-view (FOV), in degrees, and the image width and height. The resulting model assumes zero skew and unit aspect (i.e. pixel densities $m_x = m_y = 1$). The resulting calibration model is expected to have\n",
        "$$u_0 = \\frac{\\text{width}}{2}, v_0 = \\frac{\\text{height}}{2}, s = 0$$\n",
        "$$f_x=f_y=\\frac{w}{2\\cdot\\tan\\left(\\frac{\\text{FOV in radians}}{2}\\right)}$$"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "From FOV (degrees) and image width and height:\n",
            "Cal3_S2[\n",
            "\t28.8675, 0, 50;\n",
            "\t0, 28.8675, 25;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "fx: 28.867513459481298, fy: 28.867513459481298, s: 0.0, u0: 50.0, v0: 25.0\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# From FOV, width, and height\n",
        "fov, w, h = 120, 100, 50\n",
        "cal3 = Cal3_S2(fov, w, h)\n",
        "print(\"From FOV (degrees) and image width and height:\")\n",
        "print(cal3)\n",
        "print(f\"fx: {cal3.fx()}, fy: {cal3.fy()}, s: {cal3.skew()}, u0: {cal3.px()}, v0: {cal3.py()}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Properties"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Main Parameters\n",
        "\n",
        "The 5 main parameters of the `Cal3_S2` model can be accessed using the following member functions:\n",
        "- `fx()`: Returns $f_x = m_xf$, which is the effective focal length $f$ of the camera scaled by the pixel density $m_x$ along the x-axis.\n",
        "- `fy()`: Returns $f_y = m_yf$, which is the effective focal length $f$ of the camera scaled by the pixel density $m_y$ along the y-axis.\n",
        "- `skew()`: Returns the skew factor $s = -f_x\\cdot \\cot(\\theta)$, where $\\theta$ is the concave angle between the two skewed image axes.\n",
        "- `px()`: Returns $u_0$, the x-coordinate of the principal point with respect to the image sensor frame.\n",
        "- `py()`: Returns $v_0$, the y-coordinate of the principal point with respect to the image sensor frame.\n",
        "- `principalPoint()`: Returns a `numpy.ndarray` with 2 elements depicting the principal point. The returned vector follow the form $(u_0, v_0)$.\n",
        "- `vector()`: Returns a `numpy.ndarray` with 5 elements depicting a vectorized form of the calibration parameters. The returned 5-vector follow the following form $(f_x, f_y, s, u_0, v_0)$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Calibration object call:\n",
            "fx: 1500.0\n",
            "fy: 1600.0\n",
            "s: 0.1\n",
            "u0: 320.0\n",
            "v0: 240.0\n",
            "Principal point (u0, v0): [320. 240.]\n",
            "Calibration vector [fx, fy, s, u0, v0]: [1.5e+03 1.6e+03 1.0e-01 3.2e+02 2.4e+02]\n"
          ]
        }
      ],
      "source": [
        "fx, fy, s, u0, v0 = 1500.0, 1600.0, 0.1, 320.0, 240.0\n",
        "cal4 = Cal3_S2(fx, fy, s, u0, v0)\n",
        "\n",
        "print(\"Calibration object call:\")\n",
        "print(f\"fx: {cal4.fx()}\")\n",
        "print(f\"fy: {cal4.fy()}\")\n",
        "print(f\"s: {cal4.skew()}\")\n",
        "print(f\"u0: {cal4.px()}\")\n",
        "print(f\"v0: {cal4.py()}\")\n",
        "print(f\"Principal point (u0, v0): {cal4.principalPoint()}\")\n",
        "print(f\"Calibration vector [fx, fy, s, u0, v0]: {cal4.vector()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### Derived Properties\n",
        "\n",
        "`Cal3_S2` also has some member functions to retrieve useful derived properties:\n",
        "- `aspectRatio()`: Returns the aspect ratio computed with $f_x / f_y$ (which is also equivalent to $m_x / m_y$).\n",
        "- `K()`: Returns a `numpy.ndarray` with a dimension of $3\\times 3$ representing the calibration matrix $K$ for the model.\n",
        "- `inverse()`: Returns the inverted calibration matrix $K^{-1}$."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "cal3s2-accessors-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Aspect ratio: 0.9375\n",
            "K matrix:\n",
            "[[1.5e+03 1.0e-01 3.2e+02]\n",
            " [0.0e+00 1.6e+03 2.4e+02]\n",
            " [0.0e+00 0.0e+00 1.0e+00]]\n",
            "inv(K) matrix:\n",
            "[[ 6.66666667e-04 -4.16666667e-08 -2.13323333e-01]\n",
            " [ 0.00000000e+00  6.25000000e-04 -1.50000000e-01]\n",
            " [ 0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n"
          ]
        }
      ],
      "source": [
        "print(f\"Aspect ratio: {cal4.aspectRatio()}\")\n",
        "print(f\"K matrix:\\n{cal4.K()}\")\n",
        "print(f\"inv(K) matrix:\\n{cal4.inverse()}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Basic Operations\n",
        "\n",
        "### `calibrate()`\n",
        "\n",
        "The `calibrate(p)` function converts a 2D point `p` in image pixel coordinates $(u, v)$ to normalized image coordinates $(x, y)$. This member function effectively implements the formula\n",
        "\n",
        "$$\n",
        "p_{\\text{norm}} = K^{-1} \\cdot p_{\\text{pixels}}\n",
        "$$\n",
        "\n",
        "where $p_{\\text{norm}}$ and $p_{\\text{pixels}}$ are homogeneous representations of $(x, y)$ and $(u, v)$, respectively. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "id": "cal3s2-calibrate-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Converting top-left pixel coordinates to normalized pixel coordinates: \n",
            "Pixel point: [0. 0.]\n",
            "Calibrated (normalized) point: [-0.32 -0.24]\n"
          ]
        }
      ],
      "source": [
        "fx, fy, s, u0, v0 = 1000.0, 1000.0, 0, 320.0, 240.0\n",
        "cal_model = Cal3_S2(fx, fy, s, u0, v0)\n",
        "\n",
        "print(\"Converting top-left pixel coordinates to normalized pixel coordinates: \")\n",
        "p_pixels = Point2(0, 0)\n",
        "print(f\"Pixel point: {p_pixels}\")\n",
        "\n",
        "p_norm = cal_model.calibrate(p_pixels)\n",
        "print(f\"Calibrated (normalized) point: {p_norm}\")"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Converting princpal point pixel coordinates to normalized pixel coordinates: \n",
            "Pixel point: [320. 240.]\n",
            "Calibrated (normalized) point: [0. 0.]\n"
          ]
        }
      ],
      "source": [
        "print(\"Converting princpal point pixel coordinates to normalized pixel coordinates: \")\n",
        "p_pixels = Point2(320, 240)\n",
        "print(f\"Pixel point: {p_pixels}\")\n",
        "\n",
        "p_norm = cal_model.calibrate(p_pixels)\n",
        "print(f\"Calibrated (normalized) point: {p_norm}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "The `calibrate()` member function can optionally compute Jacobians with respect to the calibration parameters (`Dcal`) and the input point (`Dp`). This is useful for optimization tasks. Note that matrices you pass in must be column-major arrays with the correct shape."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Jacobian Dcal_calibrate:\n",
            "[[-0.     0.    -0.    -0.001  0.   ]\n",
            " [ 0.    -0.     0.     0.    -0.001]]\n",
            "Jacobian Dp_calibrate:\n",
            "[[ 0.001 -0.   ]\n",
            " [ 0.     0.001]]\n"
          ]
        }
      ],
      "source": [
        "# Jacobians for calibrate(p_pixels)\n",
        "Dcal_calibrate = np.zeros((2, 5), order='F')\n",
        "Dp_calibrate = np.zeros((2, 2), order='F')\n",
        "_ = cal_model.calibrate(p_pixels, Dcal_calibrate, Dp_calibrate) # Calibrated point is returned, assign to _\n",
        "print(f\"Jacobian Dcal_calibrate:\\n{Dcal_calibrate}\")\n",
        "print(f\"Jacobian Dp_calibrate:\\n{Dp_calibrate}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "### `uncalibrate()`\n",
        "\n",
        "The `uncalibrate(p)` member function converts a 2D point `p` from normalized image coordinates $(u, v)$ back to image pixel coordinates $(u, v)$. This is the inverse operation of `calibrate()` and effectively implements the formula\n",
        "\n",
        "$$\n",
        "p_{\\text{pixels}} = K \\cdot p_{\\text{norm}}\n",
        "$$"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "metadata": {
        "id": "cal3s2-uncalibrate-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Normalized point: [0. 0.]\n",
            "Uncalibrated (pixel) point: [320. 240.]\n"
          ]
        }
      ],
      "source": [
        "p_pixels_recovered = cal_model.uncalibrate(p_norm)\n",
        "print(f\"Normalized point: {p_norm}\")\n",
        "print(f\"Uncalibrated (pixel) point: {p_pixels_recovered}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cal3s2-jacobian-header"
      },
      "source": [
        "The `uncalibrate()` member function can also optionally compute Jacobians with respect to the calibration parameters (`Dcal`) and the input point (`Dp`). Likewise, the matrices you pass in must be column-major arrays with the correct shape."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 11,
      "metadata": {
        "id": "cal3s2-jacobian-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Jacobian Dcal_calibrate:\n",
            "[[0. 0. 0. 1. 0.]\n",
            " [0. 0. 0. 0. 1.]]\n",
            "Jacobian Dp_calibrate:\n",
            "[[1000.    0.]\n",
            " [   0. 1000.]]\n"
          ]
        }
      ],
      "source": [
        "# Jacobians for calibrate(p_norm)\n",
        "Dcal_calibrate = np.zeros((2, 5), order='F')\n",
        "Dp_calibrate = np.zeros((2, 2), order='F')\n",
        "_ = cal_model.uncalibrate(p_norm, Dcal_calibrate, Dp_calibrate) # Calibrated point is returned, assign to _\n",
        "print(f\"Jacobian Dcal_calibrate:\\n{Dcal_calibrate}\")\n",
        "print(f\"Jacobian Dp_calibrate:\\n{Dp_calibrate}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cal3s2-manifold-header"
      },
      "source": [
        "### Manifold Operations\n",
        "\n",
        "`Cal3_S2`, like many geometric types in GTSAM, is treated as a manifold in order to optimize over it. This means it supports operations like `retract()` (moving on the manifold given a tangent vector) and `localCoordinates()` (finding the tangent vector between two points on the manifold). These operations enable gradient-based optimization by applying updates in the tangent space of the manifold."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 12,
      "metadata": {
        "id": "cal3s2-manifold-code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Original cal_model:\n",
            "Cal3_S2[\n",
            "\t1000, 0, 320;\n",
            "\t0, 1000, 240;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "Delta vector: [10.   20.    0.05  1.   -1.  ]\n",
            "Retracted cal_retracted:\n",
            "Cal3_S2[\n",
            "\t1010, 0.05, 321;\n",
            "\t0, 1020, 239;\n",
            "\t0, 0, 1\n",
            "]\n",
            "\n",
            "\n",
            "Local coordinates from cal_model to cal_retracted:\n",
            "[10.   20.    0.05  1.   -1.  ]\n"
          ]
        }
      ],
      "source": [
        "print(\"Original cal_model:\")\n",
        "print(cal_model)\n",
        "\n",
        "# Retract: Apply a delta to the calibration parameters\n",
        "delta_vec = np.array([10.0, 20.0, 0.05, 1.0, -1.0])\n",
        "cal_retracted = cal_model.retract(delta_vec)\n",
        "print(f\"Delta vector: {delta_vec}\")\n",
        "print(\"Retracted cal_retracted:\")\n",
        "print(cal_retracted)\n",
        "\n",
        "# Local coordinates: Find the delta between two calibrations\n",
        "local_coords = cal_model.localCoordinates(cal_retracted)\n",
        "print(\"\\nLocal coordinates from cal_model to cal_retracted:\")\n",
        "print(local_coords)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Additional Resources\n",
        "\n",
        "The following curated resources provide detailed explanations of the camera calibration process. If you found any parts of this user guide to be confusing, we recommend getting started by reviewing these resources first:\n",
        "\n",
        "### Article(s)\n",
        "- [\"Camera Intrinsics: Axis skew\"](https://blog.immenselyhappy.com/post/camera-axis-skew/) by Ashima Athri\n",
        "\n",
        "### Video(s)\n",
        "- [\"Linear Camera Model | Camera Calibration\"](https://youtu.be/qByYk6JggQU?si=t8kTo_GRWYjs5f53) by Dr. Shree Nayar from Columbia University\n",
        "- [\"Computer Vision: The Camera Matrix\"](https://youtu.be/Hz8kz5aeQ44?si=Y823_mlZoJOV0gyW) by Michael Prasthofer\n",
        "\n",
        "## Source\n",
        "- [Cal3_S2.h](https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/Cal3_S2.h)\n",
        "- [Cal3_S2.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/Cal3_S2.cpp)"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": []
    },
    "kernelspec": {
      "display_name": "base",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.10"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}